//=============================================================================
//
// MotorStop.cpp :
//
// This example shows how to drive an encoder motor on M1. It uses the 
// ultrasonic sensor on I1 to control the motor operation. If no obstacle is 
// "seen" by the ultrasonic sensor, the motor runs. If the value of I1 falls 
// below 15, the motor stops. After each stop the reached counter value on C1 
// is displayed after which the counter is reset to 0 for the next run.
//
//-----------------------------------------------------------------------------
// Disclaimer - Exclusion of Liability
//
// This software is distributed in the hope that it will be useful,but WITHOUT 
// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
// FITNESS FOR A PARTICULAR PURPOSE. It can be used an modified by anyone
// free of any license obligations or authoring rights.
//=============================================================================

#include "StdAfx.h"

enum MotorIdx {MOTOR_1 = 0, MOTOR_2, MOTOR_3, MOTOR_4};
enum InpIdx {I1 = 0, I2, I3, I4, I5, I6, I7, I8};
enum CntIdx {COUNT_1 = 0, COUNT_2, COUNT_3, COUNT_4};

#define MOTOR_ON    TRUE
#define MOTOR_OFF   FALSE

#define STRMAXLEN   80

HANDLE  fthdl;
DWORD   errCode;
char    ComPortName[STRMAXLEN];
char    LibVersion[STRMAXLEN];


//=============================================================================

/*-----------------------------------------------------------------------------
 *  MotorStop  
 *---------------------------------------------------------------------------*/
static void MotorStop(void) {

    INT16   distance, count, cntmode;
    BOOL32  overrun;
    BOOL32  MotorStopped;
    int     duty;

    //  set Motor 1 mode as ON, run motor with duty=DUTY_MIN (i.e. stop it)
    SetOutMotorValues(fthdl, TA_LOCAL, MOTOR_1, DUTY_MIN, 0);
    SetFtMotorConfig(fthdl, TA_LOCAL, MOTOR_1, MOTOR_ON);

    //  set Input I1 to 'Ultrasonic sensor' mode
    SetFtUniConfig(fthdl, TA_LOCAL, I1, MODE_ULTRASONIC, TRUE);

    //  reset counter 1 (index=0)
    cout << "reset counter 1...";
    StartCounterReset(fthdl, TA_LOCAL, COUNT_1);

    //  waiting for counter reset
    do {
        GetInCounterValue(fthdl, TA_LOCAL, COUNT_1, &count, &cntmode);
        Sleep(1);
    } while (count > 0);
    cout << "OK" << endl;


    //  init value
    MotorStopped = false;
    duty = DUTY_MAX;

    //  endless loop...
    //  if value of I1 (distance) is smaller than 15, the encoder motor stop,
    //  otherwise the motor runs with max speed
    while (1) {

        //  read input value from Transfer Area
        GetInIOValue(fthdl, TA_LOCAL, I1, &distance, &overrun);

        //  set duty
        if (distance < 15) {
            MotorStopped = (duty == DUTY_MAX);
            duty = DUTY_MIN;
        }
        else {
            duty = DUTY_MAX;
        }

        //  set motor values
        SetOutMotorValues(fthdl, TA_LOCAL, MOTOR_1, duty, 0);

        if (MotorStopped) {

            //  read counter value from Transfer Area
            GetInCounterValue(fthdl, TA_LOCAL, COUNT_1, &count, &cntmode);

            //  print the distance: value of counter 1
            cout << "Motor stopped, distance= " << distance << ", counter= " << count << endl;

            //  reset counter 1 (index=0)
            StartCounterReset(fthdl, TA_LOCAL, COUNT_1);

            //  reset flag
            MotorStopped = false;
        }

        Sleep(10);
    }
}

/*-----------------------------------------------------------------------------
 *  InitTAValues  
 *---------------------------------------------------------------------------*/
static void InitTAValues() {

    //  configure all motor outputs to be used as a motor outputs and
    //  stop all motors (duty=DUTY_MIN)
    for (int mtrIdx=0; mtrIdx < N_MOTOR; mtrIdx++) {
        SetOutMotorValues(fthdl, TA_LOCAL, mtrIdx, DUTY_MIN, 0);
        SetFtMotorConfig(fthdl, TA_LOCAL, mtrIdx, MOTOR_ON);
    }
}

/*-----------------------------------------------------------------------------
 *  CheckParameter  
 *---------------------------------------------------------------------------*/
static int CheckCOMPar(int argc, char *argv[]) {

    char    *pStr;
    int     comNo;

    if (argc >= 2) {
        if (strlen(argv[1]) > 3) {
            if ((pStr=strstr(argv[1],"COM")) != NULL) {
                sscanf(pStr+3, "%d", &comNo);
                if (comNo >= 1 && comNo <= 255)
                    return 0;
                else {
                    cout << "MotorStop.exe: invalid COM number..." << endl << endl;
                    return 1;
                }
            }
        }
    }

    cout << "MotorStop.exe: no input given..." << endl << endl;
    return 1;
}

/*-----------------------------------------------------------------------------
 *  main
 *  
 *---------------------------------------------------------------------------*/
int main(int argc, char *argv[]) {

    cout << "\nExample MotorStop.exe ..." << endl;

    //  check input paramter
    if (CheckCOMPar(argc,argv)) {
        cout << "Usage: MotorStop.exe COMxx\t(e.g. COM2 or COM32)" << endl;
        return 1;
    }

    //  get library version
    ftxGetLibVersionStr(LibVersion, STRMAXLEN);
    cout << "\nftMscLib " << LibVersion << endl;

    //  library initialization
    errCode = ftxInitLib();

    strcpy(ComPortName, argv[1]);
    cout << "\nOpen ComPort '" << ComPortName << "' ..." << endl;

    //  open COM port
    fthdl = ftxOpenComDevice(ComPortName, 38400, &errCode);

    if (errCode == FTLIB_ERR_SUCCESS) {

        cout << "Connected to ROBO TX Controller ..." << endl;

        //  init values in Transfer Area to default
        InitTAValues();

        //  starting Transfer Area
        errCode = ftxStartTransferArea(fthdl);

        if (errCode == FTLIB_ERR_SUCCESS) {

            cout << "Transfer Area was started and runs..." << endl;

            MotorStop();

            //  stop Transfer Area
            ftxStopTransferArea(fthdl);
        }

        else {
            //  error case
            cout << "Error: Transfer Area was not started !" << endl;
        }

        //  closing port
        cout << "Closing ComPort '" << ComPortName << "' ..." << endl;
        errCode = ftxCloseDevice(fthdl);
    }

    else {
        //  error case
        cout << "Error: No interface available (Port '" << ComPortName << "')" << endl;
    }

    //  close library
    ftxCloseLib();

    return 0;
}
